#include "StdAfx.h"
#include "Bone.h"
#include "Animation.h"
#include "Camera.h"
#include "HelperFuncs.h"
#include "ModelNode.h"

namespace rkt
{
	void Bone::setName(const char *name)
	{
		m_name = name;
	}

	char* Bone::getName()
	{
		return (char*)m_name.c_str();
	}

	void Bone::setNotCalculated()
	{
		m_bCalced = false;
	}

	void 	Bone::setBillboard(BillboardType billboardType)
	{
		m_billboardType = billboardType;
	}
	
	void Bone::setPivotRotation(bool pivotRotation)
	{
		m_bPivotRotation = pivotRotation;
	}

	void 	Bone::setPivot(const Vector3& vPivot)
	{
		m_vPivot = vPivot;
		m_vTransformedPivot = m_vPivot;
	}

	void Bone::setPrecomputeMatrix(const Matrix4& mtx)
	{
		m_precomputeMtx = mtx;
	}

	void 	Bone::setParent(Bone *pBone)
	{
		m_pParent = pBone;
	}

	Bone*	Bone::getParent()
	{
		return m_pParent;
	}

	const Vector3&	Bone::getPivot()
	{
		return m_vPivot;
	}

	const Vector3&	Bone::getTransformedPivot()
	{
		return m_vTransformedPivot;
	}

	void Bone::calcMatrix_NoPivotRotation(IRenderSystem *pRenderSystem,AnimationTime* pTime,int rangeIndex)
	{
		if(m_bCalced)return;

		Matrix4 m;
		Quaternion q;

		m = Matrix4::IDENTITY;

		//translation
		Vector3 translation = m_pTranslation->getFrame(rangeIndex,pTime);
		m[0][3] += translation.x;
		m[1][3] += translation.y;
		m[2][3] += translation.z;

		//rotation
		q = m_pRotation->getFrame(rangeIndex,pTime);
		m = m * q;

		//scale
		Vector3 scale = m_pScale->getFrame(rangeIndex,pTime);
		m = m * Matrix4::getScale(scale);

		if(m_billboardType != BILLBOARD_NOT_USED)
		{
			Matrix4 mbb = calcBillboardMatrix(pRenderSystem->getModelViewMatrix(),m_vPivot,m_billboardType);
			m = m * mbb;
		}

		/////////////////////////
		//float (*) ptm[4][4] = &( (*m_pMKTM)[0].tm );
		/*int idx = 0;
		for(int i=0;i<(*m_pMKTM).size();i++)
		{
			if((*m_pMKTM)[i].time>pTime->current)
				break;
			idx++;
		}
		Matrix4 mtest(( (*m_pMKTM)[idx].tm )[0][0],( (*m_pMKTM)[idx].tm )[0][1],( (*m_pMKTM)[idx].tm )[0][2],( (*m_pMKTM)[idx].tm )[0][3],( (*m_pMKTM)[idx].tm )[1][0],( (*m_pMKTM)[idx].tm )[1][1],( (*m_pMKTM)[idx].tm )[1][2],( (*m_pMKTM)[idx].tm )[1][3],
			( (*m_pMKTM)[idx].tm )[2][0],( (*m_pMKTM)[idx].tm )[2][1],( (*m_pMKTM)[idx].tm )[2][2],( (*m_pMKTM)[idx].tm )[2][3],( (*m_pMKTM)[idx].tm )[3][0],( (*m_pMKTM)[idx].tm )[3][1],( (*m_pMKTM)[idx].tm )[3][2],( (*m_pMKTM)[idx].tm )[3][3]);
		m_mtxTransform = mtest;*/
///////////////////////////////////////////////////////////////////////
		
		//mtest = mtest.transpose();

		q = m.extractQuaternion();
		if(m_pParent)
		{
			m_pParent->calcMatrix(pRenderSystem,pTime,rangeIndex);
			m_mtxTransform = m_pParent->m_mtxTransform * m;
			//m_mtxTransform = m_pParent->m_mtxTransform * mtest;
		}
		else
		{
			m_mtxTransform = m;
			//m_mtxTransform = mtest;
		}

		if(m_pParent)
		{
			m_quatRotation = m_pParent->m_quatRotation * q;
		}
		else
		{
			m_quatRotation = q;
		}
	
		m_vTransformedPivot = m_mtxTransform * m_vPivot;

		
		m_bCalced = true;
	}
//
//	void printMatrix(const Matrix4& mtx)
//	{
//#if 0
//		for(int i = 0;i < 4;i++)
//		{
//			for(int j = 0;j < 4;j++)
//			{
//				char str[256];
//				sprintf(str,"%f,",mtx[j][i]);
//				OutputDebugString(str);
//			}
//			OutputDebugString("\n");
//		}
//		OutputDebugString("\n");
//#endif
//	}
//
//	Matrix4* MatrixTransformation( 
//		Matrix4 *pOut, 
//		const Vector3 *pScalingCenter, 
//		const Quaternion *pScalingRotation, 
//		const Vector3 *pScaling, 
//		const Vector3 *pRotationCenter, 
//		const Quaternion *pRotation,
//		const Vector3 *pTranslation)
//	{
//		Matrix4 m1, m2, m3, m4 = Matrix4::IDENTITY, m5, m6 = Matrix4::IDENTITY, m7, p1, p2, p3, p4, p5;
//		Vector3 prc;
//		Vector3 psc, pt;
//
//		if ( !pScalingCenter )
//		{
//			psc.x = 0.0f;
//			psc.y = 0.0f;
//			psc.z = 0.0f;
//		}
//		else
//		{
//			psc.x = pScalingCenter->x;
//			psc.y = pScalingCenter->y;
//			psc.z = pScalingCenter->z;
//		}
//		if ( !pRotationCenter )
//		{
//			prc.x = 0.0f;
//			prc.y = 0.0f;
//			prc.z = 0.0f;
//		}
//		else
//		{
//			prc.x = pRotationCenter->x;
//			prc.y = pRotationCenter->y;
//			prc.z = pRotationCenter->z;
//		}
//		if ( !pTranslation )
//		{
//			pt.x = 0.0f;
//			pt.y = 0.0f;
//			pt.z = 0.0f;
//		}
//		else
//		{
//			pt.x = pTranslation->x;
//			pt.y = pTranslation->y;
//			pt.z = pTranslation->z;
//		}
//		m1.makeTrans(-psc.x,-psc.y,-psc.z);
//		if ( !pScalingRotation )
//		{
//			m2 = Matrix4::IDENTITY;
//			m4 = Matrix4::IDENTITY;
//		}
//		else
//		{
//			Matrix3 m;
//			pScalingRotation->ToRotationMatrix(m);
//			m4 = m;
//			m2 = m3.inverse();
//		}
//		if ( !pScaling )
//		{
//			m3 = Matrix4::IDENTITY;
//		}
//		else
//		{
//			m3 = Matrix4::getScale(*pScaling);
//		}
//		if ( !pRotation )
//		{
//			m6 = Matrix4::IDENTITY;
//		}
//		else
//		{
//			Matrix3 m;
//			pRotation->ToRotationMatrix(m);
//			m6 = m;
//		}
//		m5.makeTrans(psc.x - prc.x,  psc.y - prc.y,  psc.z - prc.z);
//		m7.makeTrans(prc.x + pt.x, prc.y + pt.y, prc.z + pt.z);
//		p1 = m2 * m1;
//		printMatrix(p1);
//		p2 = m3 * p1;
//		printMatrix(p2);
//		p3 = m4 * p2;
//		printMatrix(p3);
//		p4 = m5 * p3;
//		printMatrix(p4);
//		p5 = m6 * p4;
//		printMatrix(p5);
//		*pOut = m7 * p5;
//		printMatrix(*pOut);
//
//		return pOut;
//	}
//
//	void Bone::calcMatrix_PivotRotation(IRenderSystem *pRenderSystem,AnimationTime* pTime,int rangeIndex)
//	{
//		if(m_bCalced)return;
//
//		//pTime->current = 567;
//		if(StringHelper::casecmp(m_name,"Mesh13") == 0)
//		{
//			pTime = pTime;
//		}
//
//		Matrix4 m;
//
//		//translation
//		Vector3 translation = m_vTranslation.getFrame(rangeIndex,pTime);
//		//rotation
//		Quaternion q = m_vRotation.getFrame(rangeIndex,pTime);
//		//scale
//		Vector3 scale = m_vScale.getFrame(rangeIndex,pTime);
//
//		MatrixTransformation(&m, &m_vPivot, NULL, &scale, &m_vPivot, &q, &translation);
//
//		if(m_billboardType != BILLBOARD_NOT_USED)
//		{
//			Matrix4 mbb = calcBillboardMatrix(pRenderSystem->getModelViewMatrix(),m_vPivot,m_billboardType);
//			m = m * mbb;
//		}
//
//		if(m_pParent)
//		{
//			m_pParent->calcMatrix(pRenderSystem,pTime,rangeIndex);
//			m_mtxTransform = m_pParent->m_mtxTransform * m;
//		}
//		else
//		{
//			m_mtxTransform = m_precomputeMtx * m;
//		}
//
//		m_quatRotation = m_mtxTransform.extractQuaternion();
//
//		m_vTransformedPivot = m_mtxTransform * m_vPivot;
//
//		m_bCalced = true;
//	}


	void Bone::_calcMatrix_PivotRotation(IRenderSystem *pRenderSystem,AnimationTime* pTime,int rangeIndex)
	{
		if(m_bCalced)return;

		if(m_pParent)
		{
			m_pParent->calcMatrix(pRenderSystem,pTime,rangeIndex);
		}

		Matrix4 m;
		Quaternion q;

		m.makeTrans(m_vPivot);

		//translation
		if(m_pTranslation->getInterpolationType() != INTERPOLATION_NONE)
		{
			Vector3 translation = m_pTranslation->getFrame(rangeIndex,pTime);
			m[0][3] += translation.x;
			m[1][3] += translation.y;
			m[2][3] += translation.z;
		}

		//rotation
		if(m_pRotation->getInterpolationType() != INTERPOLATION_NONE)
		{
			q = m_pRotation->getFrame(rangeIndex,pTime);
			m = m * q;
		}

		//scale
		if(m_pScale->getInterpolationType() != INTERPOLATION_NONE)
		{
			Vector3 scale = m_pScale->getFrame(rangeIndex,pTime);
			m = m * Matrix4::getScale(scale);
		}

		if(m_billboardType != BILLBOARD_NOT_USED)
		{
			Matrix4 mtx = pRenderSystem->getModelViewMatrix();
			if(m_pNode)
			{
				mtx = mtx * m_pNode->getFullTransform();
			}
			Matrix4 mbb = calcBillboardMatrix(mtx,m_vPivot,m_billboardType);
			m = m * mbb;
		}

		Matrix4 m4;
		m4.makeTrans(-m_vPivot);

		m = m * m4;

		if(m_pParent)
		{
			m_mtxTransform = m_pParent->m_mtxTransform * m;
		}
		else
		{
			m_mtxTransform = m;
		}

		m_quatRotation = m_mtxTransform.extractQuaternion();

		m_vTransformedPivot = m_mtxTransform * m_vPivot;

		m_bCalced = true;
	}

	void  Bone::calcMatrix(IRenderSystem *pRenderSystem,AnimationTime* pTime,int rangeIndex)
	{
		m_bPivotRotation ? _calcMatrix_PivotRotation(pRenderSystem,pTime,rangeIndex) : calcMatrix_NoPivotRotation(pRenderSystem,pTime,rangeIndex);
	}

	/*void Bone::addKeyFrame(const KeyFrame<Vector3>& translation,const KeyFrame<Quaternion>& rotation,const KeyFrame<Vector3>& scale)
	{
		m_vTranslation.addKeyFrame(translation);
		m_vRotation.addKeyFrame(rotation);
		m_vScale.addKeyFrame(scale);
	}

	int	Bone::addKeyFrameRange(const KeyFrameTimeRange& keyFrameTimeRange)
	{
		size_t numT = m_vTranslation.numKeyFrames();
		size_t numR = m_vRotation.numKeyFrames();
		size_t numS = m_vScale.numKeyFrames();

		if(numT != numR || numT != numS)return -1;

		m_vTranslation.addKeyFrameRange(keyFrameTimeRange);
		m_vRotation.addKeyFrameRange(keyFrameTimeRange);
		return m_vScale.addKeyFrameRange(keyFrameTimeRange);
	}
	
	void Bone::setInterpolationType(InterpolationType translation,InterpolationType rotation,InterpolationType scale)
	{
		m_vTranslation.setInterpolationType(translation);
		m_vRotation.setInterpolationType(rotation);
		m_vScale.setInterpolationType(scale);
	}


	void Bone::addKeyFrameTranslation(const KeyFrame<Vector3>& keyFrame)
	{
		m_vTranslation.addKeyFrame(keyFrame);
	}

	void Bone::addKeyFrameRotation(const KeyFrame<Quaternion>& keyFrame)
	{
		m_vRotation.addKeyFrame(keyFrame);
	}

	void Bone::addKeyFrameScale(const KeyFrame<Vector3>& keyFrame)
	{
		m_vScale.addKeyFrame(keyFrame);
	}

	void Bone::setInterpolationTypeTranslation(InterpolationType interpolationType)
	{
		m_vTranslation.setInterpolationType(interpolationType);
	}

	void Bone::setInterpolationTypeRotation(InterpolationType interpolationType)
	{
		m_vRotation.setInterpolationType(interpolationType);
	}

	void Bone::setInterpolationTypeScale(InterpolationType interpolationType)
	{
		m_vScale.setInterpolationType(interpolationType);
	}*/

	void Bone::setKeyFramesTranslation(KeyFrames<Vector3>& keyFrames)
	{
		m_pTranslation = &keyFrames;
	}

	void Bone::setKeyFramesRotation(KeyFrames<Quaternion>& keyFrames)
	{
		m_pRotation = &keyFrames;
	}

	void Bone::setKeyFramesScale( KeyFrames<Vector3>& keyFrames)
	{
		m_pScale = &keyFrames;
	}
}